//
// Created by LiuDongPeng on 2023/11/16.
//

#include "motor.h"
#include "foc.h"

#include "tim.h"
#include "bsp.h"
#include "lowpass_filter.h"


int motor_create(motor_t *motor)
{
    if (motor == NULL)
        return -1;

    memset(motor, 0, sizeof(motor_t));

    motor->id = 1;

    motor->status = MOTOR_STATUS_STOP;

    motor->udc = 24.0f;

    motor->pwmParam.period = 1e-4f;
//    motor->pwmParam.Tpwm = 16000;
    motor->pwmParam.Tpwm = 2 * MOTOR1_TIM1_ARR;

    motor->theta = 0.0f;
    motor->angleRadOffset = 681.0f / 16384.0f * (float)M_TWOPI; // 2806云台电机
//    motor->angleRadOffset = 275.0f / 16384.0f * (float)M_TWOPI; // 2312A电机
//    motor->angleRadOffset = 273.0f / 16384.0f * (float)M_TWOPI; // 2804云台电机
//    motor->angleRadOffset = 2153.0f / 16384.0f * (float)M_TWOPI; // 5010电机

    /* 电机参数 */
    motor->polePairNum = 7;
    motor->Rs = 3.2f / 2.0f; // 2806云台电机
//    motor->Rs = 0.181f / 2.0f; // 2312A电机
//    motor->Rs = 1.9f / 2.0f; // 2804云台电机
//    motor->Rs = 0.12f; // 5010电机

    motor->Ld = 50.0f / 1e6f; // 5010电机
    motor->Ld = 0.26f / 1000.0f;  // 2806云台电机

	/* 相电流低通滤波器初始化 */
	low_pass_filter_init(&motor->iaLpFilter, 0.2f);
	low_pass_filter_init(&motor->icLpFilter, 0.2f);

    /* id iq pid参数 */
    float smapleFreq = 20e3f;
	float bandwidth = fminf(20e3f * 0.25f, 1000.0f);
    float kp = motor->Ld * (float)M_TWOPI * smapleFreq / 20.0f;
    float ki = motor->Rs / (motor->Ld * smapleFreq);
    float upLimit = motor->udc / SQRT_3 * 0.8f;
    float lowLimit = -upLimit;
	pid_ctrl_init(&motor->dCurrentPid, kp, ki, 0.0f, 20.0f, lowLimit, upLimit);
	pid_ctrl_init(&motor->qCurrentPid, kp, ki, 0.0f, 20.0f, lowLimit, upLimit);

    /* id iq 低通滤波器初始化 */
    low_pass_filter_init(&motor->idLpFilter, 0.1f);
    low_pass_filter_init(&motor->iqLpFilter, 0.1f);

    /* 电机转速 PID控制器、低通滤波器初始化 */
    float iqLimit = 2.0f;
    pid_ctrl_init(&motor->speedPid, 0.005f, 0.000025f, 0.0f, 20.0f, -iqLimit, iqLimit);
    low_pass_filter_init(&motor->speedLdFilter, 0.3f);

    /* 电机位置PID控制器初始化 */
    float velLimit = 600.0f;
    pid_ctrl_init(&motor->positionPid, 20.5f, 0.001f, 0, 200.0f, -velLimit, velLimit);

    return 0;
}

void motor_enable(motor_t *motor)
{
    if (motor == NULL)
        return;

    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2);
    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3);
    HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_1);
    HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_2);
    HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_3);
}

void motor_disable(motor_t *motor)
{
    if (motor == NULL)
        return;

    HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_1);
    HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_2);
    HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_3);
    HAL_TIMEx_PWMN_Stop(&htim1, TIM_CHANNEL_1);
    HAL_TIMEx_PWMN_Stop(&htim1, TIM_CHANNEL_2);
    HAL_TIMEx_PWMN_Stop(&htim1, TIM_CHANNEL_3);
}

/**
 * @brief Set motor status
 * @param[out]  motor
 * @param[in]   status
 */
void motor_set_status(motor_t *motor, motor_status_t status)
{
    if (motor != NULL)
    {
        motor->status = status;
    }
}

/**
 * @brief Check if the motor has an encoder
 * @param motor
 * @return
 */
bool motor_has_encoder(motor_t *motor)
{
    return motor->encoder != NULL;
}

/**
 * @brief Link encoder to get motor angle
 * @param motor
 * @param encoder
 * @return
 */
bool motor_link_encoder(motor_t *motor, encoder_t *encoder)
{
    if (motor == NULL || encoder == NULL)
        return false;

    motor->encoder = encoder;
    return true;
}

/**
 * @brief Get motor angle, record tick and angle
 * @param motor
 * @return
 */
float32_t motor_get_angle_rad(motor_t *motor)
{
    if (motor != NULL && motor_has_encoder(motor))
    {
        /* 记录微秒时钟滴答和角度，用于电机转速计算 */
//        motor->lastTick = bsp_get_us_tick();
        motor->lastAngleRad = motor->angleRad;

        /* 获取此刻的角度 */
        int ret = motor->encoder->encoderGetDataCallback(motor->encoder,
                                                         &motor->encoderRawData, NULL, &motor->angleRad);

        return (float)ret;
    }
    return 0;
}

/**
 * @brief Get motor velocity in r/min
 * @param motor
 * @return
 */
float32_t motor_get_speed_rpm(motor_t *motor)
{
    if (motor == NULL)
        return 0;

    /* 计算间隔时间 */
	uint32_t tick = bsp_get_us_tick();
	uint32_t detTick = 0;
	float ts = 0;
    if (tick < motor->lastTick)
        detTick = UINT16_MAX - motor->lastTick + tick;
    else
        detTick = tick - motor->lastTick;
    ts = (float)detTick * 1e-6f;

    /* 计算角度差值 */
	float detAngle = motor->angleRad - motor->lastAngleRad;
    if (motor->angleRad < motor->lastAngleRad)
        detAngle = (float)M_TWOPI - (motor->lastAngleRad - motor->angleRad);

	/* 记录此次时间戳 */
	motor->lastTick = detTick;

    motor->speedRpm = (float)(detAngle / M_TWOPI) / ts / 6.0f;
    return motor->speedRpm;
}

/**
 * @brief
 * @param motor
 */
void motor_svpwm(motor_t *motor)
{
    if (motor == NULL)
        return;

    foc_svpwm(motor->alphabetaVoltage.ualpha, motor->alphabetaVoltage.ubeta, motor->udc, motor->pwmParam.Tpwm,
              &motor->pwmParam.ta, &motor->pwmParam.tb, &motor->pwmParam.tc);

    motor->pwmParam.ccra = (uint32_t) (motor->pwmParam.ta);
    motor->pwmParam.ccrb = (uint32_t) (motor->pwmParam.tb);
    motor->pwmParam.ccrc = (uint32_t) (motor->pwmParam.tc);

	motor_set_pwm_duty(motor);
}

void motor_svpwm1(motor_t *motor)
{
    if (motor == NULL)
        return;

    float ualpha = motor->alphabetaVoltage.ualpha;
    float ubeta = motor->alphabetaVoltage.ubeta;
    float udc = motor->udc;

    float min, sumj, sumb;
    min = -0.5f * ualpha;
    sumj = 0.866025388f * ubeta;
    sumb = min + sumj;
    sumj = min - sumj;

    min = (fminf(fminf(ualpha, sumb), sumj) + fmaxf(fmaxf(ualpha, sumb), sumj)) * -0.5f;

    float ccr1 = (-(min + ualpha) / udc + 0.5f) * ((float)motor->pwmParam.Tpwm * 0.5f);
    float ccr2 = (-(min + sumb) / udc + 0.5f) * ((float)motor->pwmParam.Tpwm * 0.5f);
    float ccr3 = (-(min + sumj) / udc + 0.5f) * ((float)motor->pwmParam.Tpwm * 0.5f);

    motor->pwmParam.ccra = (uint32_t) (ccr1);
    motor->pwmParam.ccrb = (uint32_t) (ccr2);
    motor->pwmParam.ccrc = (uint32_t) (ccr3);

	motor_set_pwm_duty(motor);
}



/**
 * @brief foc算法
 */
void motor_foc_algorithm_step(motor_t *motor)
{
    if (motor == NULL)
        return;

    /* 0. Get motor electricity angle */
//    motor_get_elec_angle(motor);

    /* 1. clark */
    foc_clark(motor->phaseCurrent.ia, motor->phaseCurrent.ib, motor->phaseCurrent.ic,
              &motor->alphabetaCurrent.ialpha, &motor->alphabetaCurrent.ibeta);

    /* 2. park */
    foc_park(motor->alphabetaCurrent.ialpha, motor->alphabetaCurrent.ibeta,
             motor->sincosTheta.sinTheta, motor->sincosTheta.cosTheta,
             &motor->dqCurrent.id, &motor->dqCurrent.iq);

    /* 3. id, iq pid ctrl */
    pid_ctrl_current_calc(&motor->dCurrentPid, 0, motor->dqCurrent.id, &motor->dqVoltage.ud);
    pid_ctrl_current_calc(&motor->qCurrentPid, 1.0f, motor->dqCurrent.iq, &motor->dqVoltage.uq);

    /* 4. inv park */
    foc_inv_park(motor->dqVoltage.ud, motor->dqVoltage.uq,
                 motor->sincosTheta.sinTheta, motor->sincosTheta.cosTheta,
                 &motor->alphabetaVoltage.ualpha, &motor->alphabetaVoltage.ubeta);

    /* 5. svpwm */
    motor_svpwm(motor);

    /* 6. pwm output */
    motor_set_pwm_duty(motor);
}

/**
 * @brief Set 3 channel pwm duty
 * @param motor
 */
void motor_set_pwm_duty(motor_t *motor)
{
    if (motor == NULL)
        return;

    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, motor->pwmParam.ccra);
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, motor->pwmParam.ccrb);
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, motor->pwmParam.ccrc);
}

/**
 * @brief Get elec angle
 * @param motor
 * @return
 */
float motor_get_elec_angle(motor_t *motor)
{
    if (motor == NULL)
        return 0;

    motor_get_angle_rad(motor);

    /**
     * @changelog 2023-12-07 14:56  修改电角度的计算方式
     */
    float val = fmodf((motor->angleRad - motor->angleRadOffset) * (float)motor->polePairNum, (float)M_TWOPI);
    motor->theta = val >= 0.0f ? val : (val + (float)M_TWOPI);
//    motor->theta = M_TWOPI - motor->theta;

    motor->sincosTheta.sinTheta = arm_sin_f32(motor->theta);
    motor->sincosTheta.cosTheta = arm_cos_f32(motor->theta);

    return motor->theta;
}


/**
 * @brief Start motor
 * @param[in]   motor
 */
void motor_startup(motor_t *motor)
{
    if (motor == NULL)
        return;

    motor->dqVoltage.ud = 1.5f;
    motor->dqVoltage.uq = 0;
    motor->theta = 0;
    foc_inv_park(motor->dqVoltage.ud, motor->dqVoltage.uq, arm_sin_f32(0.0f), arm_cos_f32(0.0f),
                 &motor->alphabetaVoltage.ualpha, &motor->alphabetaVoltage.ubeta);
    motor_svpwm(motor);
    motor_set_pwm_duty(motor);

    motor->status = MOTOR_STATUS_RUN;
}


/**
 * @brief Calibrate encoder
 * @param motor
 * @param elecZeroAngleRad
 * @param polePair
 */
void motor_calib_encoder(motor_t *motor, float32_t *elecZeroAngleRad, int polePair)
{
    if (motor == NULL)
        return;

    float a, b, c, d, e, f, g;
    int index = 0;
    a = elecZeroAngleRad[index++];
    b = elecZeroAngleRad[index++];
    c = elecZeroAngleRad[index++];
    d = elecZeroAngleRad[index++];
    e = elecZeroAngleRad[index++];
    f = elecZeroAngleRad[index++];
    g = elecZeroAngleRad[index++];

    /* Get current angle */
    motor->angleRad = motor_get_angle_rad(motor);

    if (motor->angleRad > a && motor->angleRad <= b)
    {
        motor->theta = (motor->angleRad - a) / (b - a) * M_TWOPI;
    }
    if (motor->angleRad > b && motor->angleRad <= c)
    {
        motor->theta = (motor->angleRad - b) / (c - b) * M_TWOPI;
    }
    if (motor->angleRad > c && motor->angleRad <= d)
    {
        motor->theta = (motor->angleRad - c) / (d - c) * M_TWOPI;
    }
    if (motor->angleRad > d && motor->angleRad <= e)
    {
        motor->theta = (motor->angleRad - d) / (e - d) * M_TWOPI;
    }
    if (motor->angleRad > e && motor->angleRad <= f)
    {
        motor->theta = (motor->angleRad - e) / (f - e) * M_TWOPI;
    }
    if (motor->angleRad > f && motor->angleRad <= g)
    {
        motor->theta = (motor->angleRad - f) / (g - f) * M_TWOPI;
    }

    if (motor->angleRad > g && motor->angleRad <= M_TWOPI)
    {
        motor->theta = (motor->angleRad - g) / (M_TWOPI - g + a) * M_TWOPI;
    }
    if (motor->angleRad > 0 && motor->angleRad <= a)
    {
        motor->theta = (motor->angleRad - g + M_TWOPI) / (M_TWOPI - g + a) * M_TWOPI;
    }
}


/**
 * @brief Align electricity zero pos
 * @param motor
 */
void motor_align_encoder(motor_t *motor, float ud, float angle)
{
    if (motor == NULL)
        return;

    motor_enable(motor);

	/* 给ud，电角度设置为0，转自会转到电气零位 */
    foc_inv_park(ud, 0, arm_sin_f32(angle), arm_cos_f32(angle),
				 &motor->alphabetaVoltage.ualpha, &motor->alphabetaVoltage.ubeta);
    motor_svpwm1(motor);

//    motor->angleRadOffset = motor_get_angle_rad(motor);

//	motor_disable(motor);
}


/**
 * @brief
 * @param motor
 * @param ud
 * @param uq
 */
void motor_open_loop_test(motor_t *motor, float32_t ud, float32_t uq)
{
    if (motor == NULL)
        return;

    /* 1. clark */
    foc_clark(motor->phaseCurrent.ia, motor->phaseCurrent.ib, motor->phaseCurrent.ic,
              &motor->alphabetaCurrent.ialpha, &motor->alphabetaCurrent.ibeta);

    /* 2. park */
    foc_park(motor->alphabetaCurrent.ialpha, motor->alphabetaCurrent.ibeta,
             motor->sincosTheta.sinTheta, motor->sincosTheta.cosTheta,
             &motor->dqCurrent.id, &motor->dqCurrent.iq);

    /* 4. inv park */
    foc_inv_park(ud, uq, motor->sincosTheta.sinTheta, motor->sincosTheta.cosTheta,
                 &motor->alphabetaVoltage.ualpha, &motor->alphabetaVoltage.ubeta);

    /* 5. svpwm */
    motor_svpwm1(motor);
}

/**
 * @brief Current closed loop
 * @param[in]   motor
 * @param[in]   idRef
 * @param[in]   iqRef
 */
void motor_current_closed_loop(motor_t *motor, float32_t idRef, float32_t iqRef)
{
    if (motor == NULL)
        return;

    /* 1. clark */
    foc_clark(motor->phaseCurrent.ia, motor->phaseCurrent.ib, motor->phaseCurrent.ic,
              &motor->alphabetaCurrent.ialpha, &motor->alphabetaCurrent.ibeta);

    /* 2. park */
    foc_park(motor->alphabetaCurrent.ialpha, motor->alphabetaCurrent.ibeta,
             motor->sincosTheta.sinTheta, motor->sincosTheta.cosTheta,
             &motor->dqCurrent.id, &motor->dqCurrent.iq);

    /* 3. id, iq pid ctrl */
    pid_ctrl_current_calc(&motor->dCurrentPid, idRef, motor->dqCurrent.id, &motor->dqVoltage.ud);
    pid_ctrl_current_calc(&motor->qCurrentPid, iqRef, motor->dqCurrent.iq, &motor->dqVoltage.uq);

    /* 4. inv park */
    foc_inv_park(motor->dqVoltage.ud, motor->dqVoltage.uq,
                 motor->sincosTheta.sinTheta, motor->sincosTheta.cosTheta,
                 &motor->alphabetaVoltage.ualpha, &motor->alphabetaVoltage.ubeta);

    /* 5. svpwm */
    motor_svpwm1(motor);
}


/**
 * @brief Speed closed loop.
 * @param[in]   motor
 * @param[in]   speedRef
 */
void motor_speed_closed_loop(motor_t *motor, float32_t speedRef)
{
    if (motor == NULL)
        return;

    pid_ctrl_speed_calc(&motor->speedPid, speedRef, motor->speedRpm, &motor->speedPid.output);
}

/**
 * @brief Speed closed loop.
 * @param[in]   motor
 * @param[in]   posRef
 */
void motor_position_closed_loop(motor_t *motor, float32_t posRef)
{
    if (motor == NULL)
        return;

    pid_ctrl_position_calc(&motor->positionPid, posRef, motor->angleRad, &motor->positionPid.output);
}